/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.templates.subsystems.ArmSubsystem;

/**
 *
 * @author matt
 */
public class SetArmPosition extends CommandBase {

    ArmSubsystem.ArmPresetLocation commandedPosition;
    private boolean m_isFinished;
    private double m_timeReachedLower;
    
    private final static double m_kArmAdjustForBridgeClimb = 0.15;
    private final static double m_kTimeToWaitLowered = 0.5;
            
    public SetArmPosition(ArmSubsystem.ArmPresetLocation position) {
        requires(armSubsystem);  
        commandedPosition = position;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        System.out.println("SetArmPosition Initialize() ====================");
        if (ArmSubsystem.ArmPresetLocation.kLowered == commandedPosition) {
            armSubsystem.setPosition(ArmSubsystem.ArmPresetLocation.kLowered);
            m_timeReachedLower = 0;
            m_isFinished = false;
            System.out.println("Dropping Arm");
        } else {
            armSubsystem.setPosition(commandedPosition);
            m_isFinished = true;
        }
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        if (!m_isFinished) {
            // once the arm is lowered, note the time
            if (armSubsystem.isLowered() && (0 == m_timeReachedLower)) {
                m_timeReachedLower = Timer.getFPGATimestamp();
                System.out.println("Arm Down Time Set");
            }

            // Once time is reached move the arm up.
            if ((0 != m_timeReachedLower)
                    && ((Timer.getFPGATimestamp() - m_timeReachedLower) > m_kTimeToWaitLowered)
                    && (oi.getCoDriverController().getButtonLeftTrigger())
                    ) {
                System.out.println("Arm Bumping Up");
                armSubsystem.adjustArm(m_kArmAdjustForBridgeClimb);
                m_isFinished = true;
            }
        }
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return m_isFinished;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}
